#pragma once;

#include "common.h"
#include <algorithm>
#include "particle.h"


class SpringBody;

class CollisionPair
{
public:
	SpringBody *a;
	SpringBody *b;

	CollisionPair()
	{
		a = 0x0;
		b = 0x0;
	}

	CollisionPair(SpringBody *a, SpringBody *b) : a(a), b(b)
	{
	}

	bool operator==(const CollisionPair &other)
	{
		if( a == other.a && b == other.b ) return true;
		if( a == other.b && b == other.a ) return true;
		return false;
	}
};

class Grid
{
private:
	struct Block
	{
		bool isModified;
		std::vector< BBox* > particles;
		int numParticles;
	};
public:
	Grid(int max_boxes, BBox worldBounds)
	{
		block_sx = 24;
		block_sy = 24;

		this->max_boxes = max_boxes;
		num_blocks = block_sx * block_sy;

		this->world_bounds.xmin = worldBounds.xmin;
		this->world_bounds.ymin = worldBounds.ymin;
		this->world_bounds.xmax = worldBounds.xmax;
		this->world_bounds.ymax = worldBounds.ymax;

		world_x_min = fabs(worldBounds.xmin);
		world_y_min = fabs(worldBounds.ymin);
		world_sizx = worldBounds.xmax - worldBounds.xmin;
		world_sizy = worldBounds.ymax - worldBounds.ymin;

		blockmap.resize( block_sx );
		for(size_t i=0; i< blockmap.size(); i++)
		{
			blockmap[i].resize( block_sy );
			for(size_t j=0; j<blockmap[i].size(); j++)
			{
				blockmap[i][j].particles.resize(max_pairs);
				blockmap[i][j].numParticles = 0;
				blockmap[i][j].isModified = false;
			}
		}

		numCollisionPairs = 0;
		collisionPairs.resize(max_pairs);

	}

	// clears collision pairs and each block in blockmap
	void clearGrid()
	{
		numCollisionPairs = 0;

		for(size_t i=0; i<block_sx; i++)
		{
			for(size_t j=0; j<block_sy; j++)
			{
				blockmap[i][j].numParticles = 0;
				blockmap[i][j].isModified = false;
			}
		}

		memset( collidedPairs, 0, sizeof(int)*max_pairs );
		//for(int i=0; i<max_pairs; i++)
		//	for(int j=0; j<max_pairs; j++)
		//		collidedPairs[i][j] = false;

	}

	// iterates through each corner in every Bounding Box and puts it into a block
	void scan()
	{
		for(size_t i=0; i<boxes.size(); i++)
		{
			BBox *b = boxes[i];
			// 4 corners of a box...
			int xTL = (int)floor( (world_x_min + boxes[i]->xmin)/(world_sizx)*block_sx );
			int yTL = (int)floor( (world_y_min + boxes[i]->ymin)/(world_sizy)*block_sy );
			int xTR = (int)floor( (world_x_min + boxes[i]->xmax)/(world_sizx)*block_sx );
			int yTR = (int)floor( (world_y_min + boxes[i]->ymin)/(world_sizy)*block_sy );
			int xBL = (int)floor( (world_x_min + boxes[i]->xmin)/(world_sizx)*block_sx );
			int yBL = (int)floor( (world_y_min + boxes[i]->ymax)/(world_sizy)*block_sy );
			int xBR = (int)floor( (world_x_min + boxes[i]->xmax)/(world_sizx)*block_sx );
			int yBR = (int)floor( (world_y_min + boxes[i]->ymax)/(world_sizy)*block_sy );

			// Set blocks modified
			setBlock(xTL, yTL, b);
			setBlock(xTR, yTR, b);
			setBlock(xBL, yBL, b);
			setBlock(xBR, yBR, b);
		}
	}

	// Helper function that sets a single block
	void setBlock(int xp, int yp, BBox *b)
	{
		xp = std::min<int>(block_sx-1, xp);
		yp = std::min<int>(block_sy-1, yp);
		xp = std::max<int>(0, xp);
		yp = std::max<int>(0, yp);
		blockmap[xp][yp].particles[ blockmap[xp][yp].numParticles ] = b;
		blockmap[xp][yp].isModified = true;
		blockmap[xp][yp].numParticles++;
	}

	// Test each boundingbox inside a block against every other
	void test(const Block &block)
	{
		for(int i=0; i<block.numParticles; i++)
		{
			BBox *a = block.particles[i];
			for(int j=i+1; j<block.numParticles; j++)
			{
				BBox *b = block.particles[j];
				if(a == b) continue; // todo, prevent a bbox to be inserted into a block more than once

				if( numCollisionPairs > max_pairs-4 ) continue;
				// if Bounds have collided this frame, don't collide them again
				
				//std::vector< CollisionPair >::iterator it;
				//it = std::find( collisionPairs.begin(), collisionPairs.end(), pair1 );
				//if( it == collisionPairs.end() ) continue;
				
				if( !collidedPairs[i][j] || !collidedPairs[j][i] )
				if( a->intersects(b) )
				{
					//CollisionPair pair1(a->parent, b->parent);
					collidedPairs[i][j] = true;
					//collidedPairs[j][i] = true; // todo, why does it cause missed pairs?
					collisionPairs[numCollisionPairs].a = a->parent;
					collisionPairs[numCollisionPairs].b = b->parent;
					numCollisionPairs++;
				}
			}
		}

	}

	// for every block, if it contains something, see if the contents collide
	void doInteractions()
	{
		for(size_t i=0; i<block_sx; i++)
		{
			for(size_t j=0; j<block_sy; j++)
			{
				if( blockmap[i][j].numParticles > 0) // more than 1 block.
				{
					test( blockmap[i][j] );
				}
			}      
		}
	}

	void addBox( BBox *aBox ) 
	{
		boxes.push_back( aBox );
	}

	void removeBox( const BBox *aBox )
	{
		boxes.erase( remove(boxes.begin(), boxes.end(), aBox),  boxes.end());
	}

	void draw();

	int max_boxes;
	int num_blocks;
	size_t block_sx;
	size_t block_sy;
	real world_x_min, world_y_min;
	real world_sizx, world_sizy;
	BBox world_bounds;
	std::vector< std::vector< Block > > blockmap;
	std::vector< BBox* > boxes;
	std::vector<  CollisionPair > collisionPairs;
	int numCollisionPairs;

	static const int max_pairs = 4096;
	int collidedPairs[max_pairs][max_pairs];
};